package naito_rescue.router;

import rescuecore2.misc.*;
import rescuecore2.misc.geometry.*;
import rescuecore2.worldmodel.*;
import rescuecore2.standard.entities.*;
import rescuecore2.misc.collections.LazyMap;

import naito_rescue.*;
import naito_rescue.agent.*;
import naito_rescue.router.object.*;
import naito_rescue.object.*;
import java.util.*;


public class AllPathsFindModule extends AbstractRoutingModule
{
	private Map<EntityID, Set<EntityID>> simpleGraph;
	
	
	public AllPathsFindModule(NAITOAgent owner, ConnectivityGraph graph){
		super(owner, graph);
		
    	StandardWorldModel world = owner.getWorldModel();
        Map<EntityID, Set<EntityID>> neighbours = new LazyMap<EntityID, Set<EntityID>>() {
            @Override
            public Set<EntityID> createValue() {
                return new HashSet<EntityID>();
            }
        };
        for (Entity next : world) {
            if (next instanceof Area) {
                Collection<EntityID> areaNeighbours = ((Area)next).getNeighbours();
                neighbours.get(next.getID()).addAll(areaNeighbours);
            }
        }
        setGraph(neighbours);
	}
	
    public void setGraph(Map<EntityID, Set<EntityID>> newGraph) {
        this.simpleGraph = newGraph;
    }
    
	public List<EntityID> constructPath(EntityID start, EntityID goal, SearchNode currentNode){
        List<EntityID> path = new LinkedList<EntityID>();
        do {
            path.add(0, currentNode.getNodeID());
            currentNode = currentNode.getParent();
            if (currentNode == null) {
                throw new RuntimeException("Found a node with no ancestor! Something is broken.");
            }
        } while (currentNode.getNodeID().getValue() != start.getValue());
        return path;
	}
	//幅優先探索ベースで、全経路の候補を探索して返す
	//とてもじゃないが、1シミュレーションステップで実行できる代物ではないと思われる
	public void search(EntityID goal, List<List<EntityID>> lists){
		// SearchNode currentNode = new SearchNode(owner.getLocation().getID(), null, -1, -1);
	
		String context = "AllPathfFindModule#search";
		System.err.println("search");
		Stack<SearchNode> stack = new Stack<SearchNode>();
		EntityID start = owner.getLocation().getID();
		stack.push(new SearchNode(start, null, -1, -1));
		Map<EntityID, Integer> passedCount = new HashMap<EntityID, Integer>(0);
		
		// スタート地点を1回訪れた状態にする
		passedCount.put(start, 1);
		//passedCount.add(start);
		while(!stack.isEmpty()){
			SearchNode currentNode = stack.pop();
			logger.debug("currentNode = " + currentNode, context);
			
			// 2回以上訪れているならば処理を中断、次のノードへ
			// ここに訪れたのが1回めならば問題無し
			if(passedCount.get(currentNode.getNodeID()) > 1){
				logger.debug("currentNode passed over 2. currentNode = " + currentNode + ", continue;", context);
				continue;
			}
			if(currentNode.getNodeID().getValue() == goal.getValue()){
				List<EntityID> list= constructPath(start, goal, currentNode);
				lists.add(list);
				logger.debug("Path is added.", context);
			}else{
				// 隣接するノードを取得
				Set<EntityID> neighbours = graph.getNeighbors(currentNode.getNodeID());
				logger.debug("currentNode = " + currentNode + ", neighbours={" + neighbours + "}", context);
				for(EntityID id : neighbours){
					if(currentNode.getParent() != null &&
						// 今辿ってきた親ノードなら次のノードへcontinue;
						currentNode.getParent().getNodeID().getValue() == id.getValue() ||
						// 2回以上訪れたノードなら次のノードへcontinue;
						(passedCount.get(id) != null &&
						passedCount.get(id) > 1)){
							if(passedCount.get(id) > 1){
								logger.debug("Neighbour " + id + " was passed over 2. continue; " + context);
							}else{
								logger.debug("Neighbour is current's parent, continue;", context);
							}
							System.err.println("continue;");
							continue;
						}
						logger.trace(passedCount.toString(), context);
						//隣接ノードを1回訪れた状態にする
						passedCount.put(id, ( passedCount.get(id)==null ? 1 : (passedCount.get(id)+1) ));
						stack.push(new SearchNode(id, currentNode, -1, -1));
				}
			}
			//passedCount.add(currentNode.getNodeID());
			EntityID currentID = currentNode.getNodeID();
			passedCount.put(currentID, ( passedCount.get(currentID)==null ? 1 : (passedCount.get(currentID)+1) ));
		}
	}
	static int count = 0;
}
